//
// Created by ycz on 2024/11/14.
//
#include <iostream>
#include <opencv2/highgui.hpp>
#include <tuw_voronoi_map/voronoi_path_generator.h>
#include <tuw_voronoi_graph/voronoi_graph_node.h>
#include <tuw_voronoi_graph/voronoi_graph_generator.h>
#include <opencv2/imgproc.hpp>

using namespace std;

int main(int argc, char **argv)
{
    cv::Mat image = cv::imread("/home/ycz/ws/exercise_ws/src/tuw_voronoi_graph/img/hospital_04.pgm", cv::IMREAD_UNCHANGED);
    threshold(image, image, 0, 255, cv::THRESH_BINARY_INV | cv::THRESH_OTSU);
//    cv::namedWindow("m0", cv::WINDOW_NORMAL);
//    cv::imshow("m0", image);
//    cv::waitKey(0);
    std::unique_ptr<float[]> potential;
    float resolution_ = 0.05;
    float segment_length_ = 1.0;
    float crossingOptimization_ = 0.2;
    float endSegmentOptimization_ = 0.2;
    cv::Mat map_, distField_, voronoiMap_;
    voronoi_map::VoronoiPathGenerator voronoiPathGenerator;
    voronoiPathGenerator.prepareMap(image, map_, 0.1 / resolution_);
//    cv::namedWindow("map_", cv::WINDOW_NORMAL);
//    cv::imshow("map_", map_);
    voronoiPathGenerator.computeDistanceField(map_, distField_);
    cv::namedWindow("distField_", cv::WINDOW_NORMAL);
    cv::imshow("distField_", distField_);
    ROS_INFO("Graph generator: Computing voronoi graph ...");
    voronoiPathGenerator.computeVoronoiMap(distField_, voronoiMap_);
    cv::namedWindow("voronoiMap_", cv::WINDOW_NORMAL);
    cv::imshow("voronoiMap_", voronoiMap_);
    ROS_INFO("Graph generator: Generating graph ...");
    potential.reset(new float[image.cols * image.rows]);
    float pixel_path_length = segment_length_ / resolution_;
    tuw_graph::VoronoiGraphGenerator voronoiGraphGenerator;
    std::vector<tuw_graph::Segment> segments_ =
            voronoiGraphGenerator.calcSegments(image, distField_, voronoiMap_, potential.get(), pixel_path_length,
                                               crossingOptimization_ / resolution_,
                                               endSegmentOptimization_ / resolution_);
    std::cout << "\033[1;32msegments_: " << segments_.size() << "\033[0m" << std::endl;
    for(auto seg : segments_)
    {
        cv::Point pt1(seg.getStart().x(), seg.getStart().y());
        cv::Point pt2(seg.getEnd().x(), seg.getEnd().y());
        std::cout << "\033[1;32msegments_: " << seg.getId() << " : " << seg.getLength()
                  << " : " << seg.getMinPathSpace() * resolution_
                  << " : " << pt1 << " : " << pt2 << "\033[0m" << std::endl;
        cv::line(map_, pt1, pt2, cv::Scalar(0, 0, 111), 1);
    }
    cv::imshow("map_", map_);
    cv::waitKey(0);
    return 0;
}

